/* ######################################################################### */
/* \file ObjectFinder.hpp
 * \brief       This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
#include <iostream>
#include <set>
/* ------------------------------------------------------------------------- */
/**     \namespace DATMO
 *
 *  \brief      This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**     \class ObjectFinder
 *
 *  \brief  This is the class of ObjectFinder function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_ObjectFinder2
{
public:
        struct Punto
        {
                int x;
                int y;
                Punto():x(0),y(0){}
                Punto(int rx, int ry):x(rx),y(ry){}
        };
        struct Punto_op
        {
                bool operator()(Punto p1, Punto p2) const
                {
                        bool res;
                        if(p1.x < p2.x) {res = true;}
                        else if (p1.x == p2.x)
                        {
                                if (p1.y <= p2.y) {res = true;}
                                else { res = false;}
                        }
                        else { res = false;}
                        return res;   // so that the set is ordered in descending order...
                }
        };
        typedef std::set<Punto,Punto_op> PuntoSet;

        /* ------------------------------------------------------------------------- */
        /**     \fn public static void ObjectDetection()
         *
         *  \brief
         *
         *  \param[in]
         *  \param[in]
         *
         *  \param[out] bool
         *
         */
        /* ------------------------------------------------------------------------- */
        cl_ObjectFinder2(){}

        static DetectedObjects ObjectFinder2(Matrix DistanceDiference)
        {
                DynObjConst str_DynObjConst = DynObjConst();
                DetectedObjects str_DetectedObjects = DetectedObjects(DistanceDiference.Nrows(),DistanceDiference.Ncols());
                int n_matrix = DistanceDiference.Nrows();
                int m_matrix = DistanceDiference.Ncols();
                Matrix Objects = Matrix(n_matrix,m_matrix);
                Objects = 0.0; //Intitialize the matrix to zeros
                int u1 [] = { 0, 1, 1, 1, 0, -1, -1, -1 };
                int u2 [] = { 1, 1, 0, -1, -1, -1, 0, 1 };
                int label = 0; //label for the objects
                int pos_i, pos_j,pos_i_act,pos_j_act;
                std::set<float> OpenPx;
                std::set<float> ClosedPx;
                std::set<int> Objects2eliminate;
                std::set<int> ObjectsLabels;
                set<int>::iterator it;
                int n_pixels = 0;
                bool cond1 = false;
                bool cond2 = false;
                bool cond3 = false;
                int min_pix = str_DynObjConst.i_min_number_pixel;

                ROS_INFO("Finder entra");

                for (int i = 1; i <= n_matrix; i++)
                {
                  for (int j = 1; j <= m_matrix; j++)
                  {
                    //If the difference is not zero (there is a object in this pixel)
                    //There is not a previous object defined for this pixel
                    //We have found a new object
                    if (DistanceDiference(i, j) != 0 && Objects(i,j) == 0)
                    {
                      //ROS_INFO("Objeto Nuevo");
                      //reset the list for open and closed pixels
                      OpenPx.clear();
                      ClosedPx.clear();
                      float act_px = (float)((i-1)*m_matrix+j);
                      label++;
                      OpenPx.insert(act_px);
                      n_pixels = 0;
                      while(OpenPx.size()>0)
                      {

                        float act_px = *OpenPx.begin();
                        pos_j_act = (int)(fmod(act_px,(float)m_matrix));
                        pos_i_act = (int)(((act_px-pos_j_act)/m_matrix)+1);
                        if (pos_j_act == 0)
                          {pos_j_act = m_matrix;}
                  //      ROS_INFO("pos i act %d - pos j act %d, i id - j %d",pos_i_act,pos_j_act,i,j);
                        //Surroundings check of the pixel
                        for (int k = 0; k < 8; k++)
                        {
                          pos_i = pos_i_act + u1[k];
                          pos_j = pos_j_act + u2[k];
                          //check if it is between boundaries
                          if ((pos_i > 0) && (pos_i <= n_matrix) && (pos_j > 0) && (pos_j <= m_matrix))
                          {
                            ROS_INFO("pos i %d - pos j %d",pos_i,pos_j);
                            float aux_pos = (pos_i-1)*m_matrix+pos_j;
                            cond1 = (OpenPx.count(aux_pos) > 0);
                            cond2 = (ClosedPx.count(aux_pos) > 0);
                            cond3 = (DistanceDiference(pos_i,pos_j)==0);
                            //check if forms part of the object
                            if (!cond1 && !cond2 && !cond3)
                            {
                              //add to the open pixels list
                              OpenPx.insert(aux_pos);
                            }
                          }
                        }
                        Objects(pos_i_act,pos_j_act) = label;
                        n_pixels++;
                        ClosedPx.insert(act_px);
                        OpenPx.erase(act_px);
                      }
                      if (n_pixels < min_pix)
                      {
                        Objects2eliminate.insert(label);
                      }
                      else
                      {
                        ObjectsLabels.insert(label);
                      }
                    }
                  }
                }
                for ( it = Objects2eliminate.begin(); it != Objects2eliminate.end(); it++)
                {
                  for (int i = 1; i <= n_matrix; i++)
                  {
                    for (int j = 1; j <= m_matrix; j++)
                    {
                      if (Objects(i,j) == *it)
                        Objects(i,j) = 0;
                    }
                  }
                  label--;
                }
                str_DetectedObjects.m_Objects = Objects;
                ROS_INFO("Fin finder con %d objetos encontrados",label);
                str_DetectedObjects.i_NumberOfObjects = label;
                str_DetectedObjects.s_labels = ObjectsLabels;
                return str_DetectedObjects;
        }
};
}
